#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>          /* See NOTES */
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netinet/if_ether.h>
#include <netinet/in.h>
#include <linux/sockios.h>
#include <net/route.h>
#include <errno.h>
#include <stdbool.h>
#include <thread>
#include <fcntl.h>
#include <sys/mman.h>
#include <pthread.h>
#include <sys/time.h>

#include "gvcpReg.h"
#include "sockgvsp.h"
#include "sockgvcp.h"
#include "gigeError.h"
#include "cam_ctrl.h"
#include "dma.h"

extern u32 acq_en;

extern u16 desPort;
extern u32 img_h;
extern char m_szRemoteIp[32];
int sockfd;

static void sock_init(int* sockfd, struct sockaddr_in* Addr)
{
    int iOptval = 1;
    *sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    setsockopt(*sockfd, SOL_SOCKET, SO_BROADCAST | SO_REUSEADDR, (char*)&iOptval, sizeof(int));
    memset(Addr, 0, sizeof(struct sockaddr_in));
    Addr->sin_family = AF_INET;
    Addr->sin_addr.s_addr = INADDR_ANY;
    Addr->sin_port = htons(9001);
    bind(*sockfd, (struct sockaddr *)Addr, sizeof(*Addr));
}

static void sock_send(int sockfd, unsigned char* rgMessage, char* m_szRemoteIp, u16 dwMsgLen, u16 dwPort)
{
    int iOptval = 1;
    struct sockaddr_in Addr;
    int bNeedClose = 0;
    if(sockfd < 0){
		sockfd = socket(AF_INET, SOCK_DGRAM, 0);
		bNeedClose=1;
	}
    setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST | SO_REUSEADDR, (char*)&iOptval, sizeof(int));
    memset(&Addr, 0, sizeof(struct sockaddr_in));
    Addr.sin_family = AF_INET;
    Addr.sin_addr.s_addr = inet_addr(m_szRemoteIp);
    Addr.sin_port = htons(dwPort);
    sendto(sockfd, rgMessage, dwMsgLen, 0, (struct sockaddr*)&Addr, sizeof(struct sockaddr));
    if(bNeedClose > 0) close(sockfd);
}

void gvsp_send_leader(u16 blockId)
{
    static u32 ttt=0;
    struct img_leader_packet *pkt = (struct img_leader_packet*)malloc(sizeof(struct img_leader_packet));
    memset(pkt, 0, sizeof(struct img_leader_packet));

    pkt->state           = 0;
    pkt->block_id        = htons(blockId);
    pkt->EIandPackformat = 1;
    pkt->packet_id_H     = htons(0);
    pkt->packet_id_L     = htons(0);
    pkt->payload_type    = htons(1);
    pkt->timestamp_H     = htonl(0);
    pkt->timestamp_L     = htonl(ttt+=0x702ca88);
    pkt->pixel_format    = htonl(0x01080001);
    pkt->size_x          = htonl(4096);
    pkt->size_y          = htonl(img_h);
    pkt->offset_x        = htonl(0);
    pkt->offset_y        = htonl(0);

    sock_send(sockfd, (unsigned char*)pkt, m_szRemoteIp, sizeof(struct img_leader_packet), desPort);
    free(pkt);
}

void gvsp_send_payload(u32 size,u32 blockId,u32 packetId)
{
    u32 len;
    len = sizeof(struct img_payload_packet_header) + size;
    struct img_payload_packet_header *pkt = (struct img_payload_packet_header*)malloc(len);
    memset(pkt, 255, len);

    pkt->state           = 0;
    pkt->block_id        = htons(blockId);
    pkt->EIandPackformat = 0x03;
    pkt->packet_id_H     = htons(packetId >> 8);
    pkt->packet_id_L     = htons(packetId);

    unsigned char* img_addr;
    int fd = open("/dev/mem", O_RDWR | O_SYNC);
    
    if(axi_read_reg(DMA_BASE_ADDR, S2MM_CURDESC >> 2) == 0x10000000){
        img_addr = (unsigned char*)mmap(NULL, 4096*img_h, PROT_READ | PROT_WRITE, MAP_SHARED, fd, (off_t)(0x10001000));
    }
    else{
        img_addr = (unsigned char*)mmap(NULL, 4096*img_h, PROT_READ | PROT_WRITE, MAP_SHARED, fd, (off_t)(0x10001000 + 4096*img_h));
    }

    memcpy(pkt->data, img_addr + (packetId-1)*size, size);
    sock_send(sockfd, (unsigned char*)pkt, m_szRemoteIp, len, desPort);
    munmap(img_addr, 4096*img_h);
    close(fd);
    free(pkt);
}

void gvsp_send_trailer(u32 blockId,u32 packetId)
{
    struct img_trailer_packet *pkt = (struct img_trailer_packet*)malloc(sizeof(struct img_trailer_packet));
    memset(pkt, 0, sizeof(struct img_trailer_packet));

    pkt->state           = 0;
    pkt->block_id        = htons(blockId);
    pkt->EIandPackformat = 0x02;
    pkt->packet_id_H     = htons(packetId >> 8);
    pkt->packet_id_L     = htons(packetId);
    pkt->reserved        = 0;
    pkt->payload_type    = htons(1);
    pkt->size_y          = htonl(img_h);

    sock_send(sockfd, (unsigned char*)pkt, m_szRemoteIp, sizeof(struct img_trailer_packet), desPort);
    free(pkt);
}

void gvsp_resend(u32 blockId, u32 firstId, u32 lastId, u32 size_pkt)
{
    //printf("RESEND %d %d %d %d\n",blockId, firstId, lastId, size_pkt);
    unsigned int size_payload = size_pkt - 36;
    unsigned int num_pkt, size_pkt_last;
    if((4096 * img_h) % size_payload == 0){
        num_pkt = 4096 * img_h/size_payload + 1;
        size_pkt_last = size_pkt;
    }
    else {
        num_pkt = 4096 * img_h/size_payload + 2;
        size_pkt_last = 4096 * img_h - (num_pkt -2)*size_payload + 36;
    }
    if(firstId > num_pkt) return;
    if(lastId > num_pkt) lastId = num_pkt;
    for(int i = firstId; i <= lastId; i++){
        if(i == 0) gvsp_send_leader(blockId);
        if(i >= 1 && i <= num_pkt - 2) gvsp_send_payload(size_payload, blockId, i);
        if(i == num_pkt - 1) gvsp_send_payload(size_pkt_last - 36, blockId, i);
        if(i == num_pkt) gvsp_send_trailer(blockId,i);
    }
}

void gvsp_loop()
{
    unsigned int *reg_addr;
    unsigned int temp = 0,intr;
    reg_addr = (unsigned int*)mmap(NULL, 65535, PROT_READ | PROT_WRITE, MAP_SHARED, open("/dev/mem", O_RDWR | O_SYNC), (off_t)CAM_STATUS_BASEADDR);

    unsigned int frame_num = 0, curr_desc = 0;
    int a;

    struct sockaddr_in Addr;
	sock_init(&sockfd, &Addr);
    
    while(1){
        intr = reg_addr[0];
        if(intr == 1 && temp == 0){
            if(acq_en == 1){
                curr_desc = axi_read_reg(DMA_BASE_ADDR, S2MM_CURDESC >> 2);
                printf("FRAME %d\n", frame_num++);
                axi_write_reg(DMA_BASE_ADDR, MM2S_CURDESC >> 2, curr_desc);
                axi_write_reg(DMA_BASE_ADDR, MM2S_DMACR >> 2, 0x11);
                axi_write_reg(DMA_BASE_ADDR, MM2S_TAILDESC >> 2, curr_desc);
            }
            else frame_num = 0;
            //printf("INTR COME\n");
            axi_write_reg(DMA_BASE_ADDR, S2MM_DMASR >> 2, 0x1000);
        }
        temp = intr;
        for(int i =1; i<=1790; i++)a=a+1;
    }
}
